#include <VolumeCounter.h>
#include <FiberSegmentExtractor.h>
#include <iostream>
#include <sstream>


#include <vnl/vnl_matrix.h>
#include <vnl/algo/vnl_levenberg_marquardt.h>


#include <vnl/vnl_least_squares_cost_function.h>
#include <vnl/vnl_least_squares_function.h>

#define OUTPUT(x) std::cout << (x) << std::endl ;

#include <CounterStatistics.h>

int main(int argc, char **argv){
  
	Counter::Initialize(0);
	Counter_WeightedVertices c ;
	
	OUTPUT(c)
  
	std::vector<AccumulateType> bins = c.GetBins() ;
	
	c.Add(geometry::Vector(1, 0, 0));
	c.Add(geometry::Vector(1, 0, 0));
	c.Add(geometry::Vector(1, 0, 0));
	c.Add(geometry::Vector(1, 0, 0));
	c.Add(geometry::Vector(1, 0, 0));
	c.Add(geometry::Vector(1, 0, 0));
	c.Add(geometry::Vector(1, 0, 0));
	c.Add(geometry::Vector(1, 0, 0));

	OUTPUT(c)
	
	SingleGaussianFunctor s(c) ;
	vnl_levenberg_marquardt lm(s) ;
	
	vnl_vector<double> param(4) ;
	param[0] = 1 ;
	param[1] = 1 ;
	param[2] = 1 ;
	param[3] = 1 ;
	
	OUTPUT(param)
	
	lm.minimize(param) ;
	
	OUTPUT(param)
	
	double phi, theta ;
	
	while(1){
		cin >> phi >> theta ;
		VectorType p = c.GetCartesianCoordinates(phi, theta) ;
		cout << p[0] << " " << p[1] << " " << p[2] << endl ;
	}
	
// 	FiberSegmentExtractor fse(argv[1]) ;
// 	VolumeCounter<Counter_WeightedVertices> vcc(4, argv[2]) ;
// 	
// 	OUTPUT("Number of fibers:")
// 	OUTPUT(fse.GetNumberOfFibers())
// 	OUTPUT("Number of segments:")
// 	OUTPUT(fse.GetNumberOfSegments())
// 	
// 	int count = 0 ;
// 	int wildCount = 0 ;
// 	while(fse){
// 		FiberSegment segment = fse.Next() ;
// 		PositionInVolume p1 = vcc.GetVoxelPositionInVolume(segment.p1) ;
// 		PositionInVolume p2 = vcc.GetVoxelPositionInVolume(segment.p2) ;
// 		if (!p1.isValid() || !p2.isValid()){
// 		        wildCount++ ;
// 			continue ;
// 		}
// 		Counter &c1 = vcc.GetCounterForResidingVoxel(segment.p1) ;
// 		Counter &c2 = vcc.GetCounterForResidingVoxel(segment.p2) ;
// 		c1.Add(segment.p1 - segment.p2) ;
// 		c1.Add(segment.p2 - segment.p1) ;
// 		c2.Add(segment.p1 - segment.p2) ;
// 		c2.Add(segment.p2 - segment.p1) ;
// 		count++ ;
// 	}
// 	
// 	OUTPUT("Finished:")
// 	OUTPUT(count)
// 	OUTPUT("Wild:")
// 	OUTPUT(wildCount)
// 	
// 	vcc.WriteVolumeCounterToNrrdFile("BKUP.nrrd") ;
// 	
// 	VolumeCounter<Counter_WeightedVertices> vc("BKUP.nrrd") ;
// 	vc.WriteVolumeCounterToNrrdFile() ;
// 	vc.WriteNumTotalVotesOfEachVoxelToNrrdFile() ;
// 	
//  	for (int i = 0; i < 3; i++){
//  		for (int j = 0; j < vc.GetNumVoxelsAlongAxis(i); j++){
//  			std::stringstream ss1, ss2 ;
//  			ss1 << "slices/" << i << "." << j << ".vtk" ;
// 			ss2 << "slices/maxima_" << i << "." << j << ".vtk" ;
//  			vc.WriteVolumeCounterSliceToVTKFile(i, j, ss1.str().c_str()) ;
// 			vc.WriteVolumeCounterSliceWithLocalMaximaMarkedToVTKFile(i, j, ss2.str().c_str()) ;
//  		}
//  	}
	
	return 0 ;
}
